
delta_l = zeros(1 ,6);
for i = 1:n
    qi = Q(i, :);
    x_real(3*i-2:3*i, 1) = robot_real.fkine(qi).t;
    M(3*i-2:3*i, :) = calculate_Jacobian(delta_l, DH_Para, qi);
end

error_x = calculate_error(delta_l, DH_Para, x_real, Q);
delta_l = M \ error_x;

robot.links(1).d = robot.links(1).d + delta_l(1);
robot.links(3).a = robot.links(3).a + delta_l(2);
robot.links(4).a = robot.links(4).a + delta_l(3);
robot.links(4).d = robot.links(4).d + delta_l(4);
robot.links(5).d = robot.links(5).d + delta_l(5);
robot.links(6).d = robot.links(6).d + delta_l(6);

disp("link offset: ");
disp(delta_l);